
#include "preprocess/map_data.h"

namespace global_path_planner {
namespace preprocess {

    MapData::MapData(){

    }

    MapData::~MapData(){
        
    }

    // 更新地图时清空地图相关信息
    void MapData::Reset(){
        rect_func_areas.clear();
        poly_func_areas.clear();
        circle_func_areas.clear();
        circle_virtue_walls.clear();
        line_virtue_walls.clear();
        rect_virtue_walls.clear();
        poly_virtue_walls.clear();
    }

    // 解析虚拟墙
    void MapData::ParseVirtualWalls(const v3_msgs::MapInfoConstPtr& msg_ptr){
        for(int i = 0; i < msg_ptr->virtual_info_list.size(); i++){
            if(msg_ptr->virtual_info_list[i].points_type == msg_ptr->virtual_info_list[i].LINE){
                // 解析线段虚拟墙
                std::vector<std::pair<float, float>> line;
                double __x0 = msg_ptr->virtual_info_list[i].line.start_pose.position.x;
                double __y0 = msg_ptr->virtual_info_list[i].line.start_pose.position.y;
                double __x1 = msg_ptr->virtual_info_list[i].line.end_pose.position.x;
                double __y1 = msg_ptr->virtual_info_list[i].line.end_pose.position.y;
                if(std::abs(__x0  - __x1) < 1e-6 &&
                    std::abs(__y0 - __y1) < 1e-6){
                    continue;
                }
                line.emplace_back(std::make_pair(__x0, __y0));
                line.emplace_back(std::make_pair(__x1, __y1));
                line_virtue_walls.emplace_back(line);
            }else if(msg_ptr->virtual_info_list[i].points_type == msg_ptr->virtual_info_list[i].CIRCLE){
                // 解析圆形虚拟墙
                double __x0 = msg_ptr->virtual_info_list[i].circle.center_pose.position.x;
                double __y0 = msg_ptr->virtual_info_list[i].circle.center_pose.position.y;
                double __z0 = msg_ptr->virtual_info_list[i].circle.radius;
                if(__z0 < 1e-6){
                    continue;
                }
                auto circle = std::make_pair(__x0, __y0);
                circle_virtue_walls.emplace_back(circle, __z0);
            }else if(msg_ptr->virtual_info_list[i].points_type == msg_ptr->virtual_info_list[i].RECTANGLE){
                // 解析矩形虚拟墙
                std::vector<std::pair<float, float>> pts;
                for(auto j = 0; j < msg_ptr->virtual_info_list[i].rectangle.poses.size(); j++){
                    double __x0 = msg_ptr->virtual_info_list[i].rectangle.poses[j].position.x;
                    double __y0 = msg_ptr->virtual_info_list[i].rectangle.poses[j].position.y;
                    if(std::fabs(__x0) < 1e-6 &&
                        std::fabs(__y0) < 1e-6){
                        continue;
                    }
                    pts.emplace_back(std::make_pair(__x0, __y0));
                }
                rect_virtue_walls.emplace_back(pts);
            }else if(msg_ptr->virtual_info_list[i].points_type == msg_ptr->virtual_info_list[i].POLYGON){
                // 解析多边形虚拟墙
                std::vector<std::pair<float, float>> pts;
                for(auto j = 0; j < msg_ptr->virtual_info_list[i].polygon.poses.size(); j++){
                    double __x0 = msg_ptr->virtual_info_list[i].polygon.poses[j].position.x;
                    double __y0 = msg_ptr->virtual_info_list[i].polygon.poses[j].position.y;
                    if(std::fabs(__x0) < 1e-6 &&
                        std::fabs(__y0) < 1e-6){
                        continue;
                    }
                    pts.emplace_back(std::make_pair(__x0, __y0));
                }
                poly_virtue_walls.emplace_back(pts);
            }
        }
    }

    // 解析功能区
    void MapData::ParseFuncAreas(const v3_msgs::MapInfoConstPtr& msg_ptr){
        // 解析不同类型的功能区
        // for(int i = 0; i < msg_ptr->functional_area_datas.size(); i++){
        //     if(msg_ptr->functional_area_datas[i].type ==
        //         std::to_string(msg_ptr->functional_area_datas[i].BLIND)){
        //             // 解析盲道功能区
        //             ParseShapeAreas(msg_ptr->functional_area_datas[i].shape, 
        //                             msg_ptr->functional_area_datas[i].BLIND,
        //                             msg_ptr->functional_area_datas[i].global_area.points);
        //     }else if(msg_ptr->functional_area_datas[i].type ==
        //         std::to_string(msg_ptr->functional_area_datas[i].SLOWDOWN)){
        //             // 解析减速带功能区
        //             ParseShapeAreas(msg_ptr->functional_area_datas[i].shape, 
        //                             msg_ptr->functional_area_datas[i].SLOWDOWN,
        //                             msg_ptr->functional_area_datas[i].global_area.points);
        //     }else if(msg_ptr->functional_area_datas[i].type ==
        //         std::to_string(msg_ptr->functional_area_datas[i].SLOPE)){
        //             // 解析斜坡功能区
        //             ParseShapeAreas(msg_ptr->functional_area_datas[i].shape, 
        //                             msg_ptr->functional_area_datas[i].SLOPE,
        //                             msg_ptr->functional_area_datas[i].global_area.points);
        //     }else if(msg_ptr->functional_area_datas[i].type ==
        //         std::to_string(msg_ptr->functional_area_datas[i].RIDGE)){
        //             // 解析坎功能区
        //             ParseShapeAreas(msg_ptr->functional_area_datas[i].shape, 
        //                             msg_ptr->functional_area_datas[i].RIDGE,
        //                             msg_ptr->functional_area_datas[i].global_area.points);
        //     }
        // }
    }

    // 解析各种形状区域
    void MapData::ParseShapeAreas(const std::string& shape, const uint8_t& type, 
                                const std::vector<geometry_msgs::Point32>& points){
        // // 解析矩形功能区
        // // 解析多边形功能区
        // if(shape == std::to_string(cln_msgs::FunctionalAreaData::RECTANGLE) ||
        //     shape == std::to_string(cln_msgs::FunctionalAreaData::POLYGON)){
        //         auto rect_pts = TwoPointsCoincide(points);
        //         if(rect_pts.size() == points.size() && 
        //             shape == std::to_string(cln_msgs::FunctionalAreaData::RECTANGLE)){
        //             rect_func_areas.emplace_back(std::make_pair(type, rect_pts));
        //         }else if(rect_pts.size() == points.size() && 
        //             shape == std::to_string(cln_msgs::FunctionalAreaData::POLYGON)){
        //             poly_func_areas.emplace_back(std::make_pair(type, rect_pts));
        //         }
        // }       
        // // 解析圆形功能区
        // else if(shape == std::to_string(cln_msgs::FunctionalAreaData::CIRCLE)){
        //     auto __x0 = points[0].x;
        //     auto __y0 = points[0].y;
        //     auto __z0 = points[0].z;
        //     if(__z0 < 1e-6) return;
        //     auto xy = std::make_pair(__x0, __y0);
        //     auto circle = std::make_pair(xy, __z0);
        //     circle_func_areas.emplace_back(type, circle);
        // }
    }

    // 判断两点是否有重合
    std::vector<std::pair<float, float>> MapData::TwoPointsCoincide(const std::vector<geometry_msgs::Point32>& points){
        std::vector<std::pair<float, float>> rect_pts;
        for(auto j = 0; j < points.size(); j++){
            float __x0, __y0, __x1, __y1;
            if(j == points.size() - 1){
                __x0 = points[j].x;
                __y0 = points[j].y;
                __x1 = points[0].x;
                __y1 = points[0].y;
            }else{
                __x0 = points[j].x;
                __y0 = points[j].y;
                __x1 = points[j + 1].x;
                __y1 = points[j + 1].y;
            }
            if(std::abs(__x0  - __x1) < 1e-6 &&
                std::abs(__y0 - __y1) < 1e-6){
                break;
            }
            rect_pts.emplace_back(std::make_pair(__x0, __y0));
        }
        return rect_pts;
    }


}  // namespace preprocess
}  // namespace global_path_planner